package pl.poznan.put.cs.idss.students.kingcart.core.tracker;

import static com.googlecode.javacv.cpp.opencv_core.CV_RGB;
import static com.googlecode.javacv.cpp.opencv_core.CV_TERMCRIT_EPS;
import static com.googlecode.javacv.cpp.opencv_core.CV_TERMCRIT_ITER;
import static com.googlecode.javacv.cpp.opencv_core.IPL_DEPTH_32F;
import static com.googlecode.javacv.cpp.opencv_core.IPL_DEPTH_8U;
import static com.googlecode.javacv.cpp.opencv_core.cvCreateImage;
import static com.googlecode.javacv.cpp.opencv_core.cvGetSize;
import static com.googlecode.javacv.cpp.opencv_imgproc.cvErode;
import static com.googlecode.javacv.cpp.opencv_imgproc.cvDilate;
import static com.googlecode.javacv.cpp.opencv_core.cvLine;
import static com.googlecode.javacv.cpp.opencv_core.cvRect;
import static com.googlecode.javacv.cpp.opencv_core.cvPoint;
import static com.googlecode.javacv.cpp.opencv_core.cvCircle;
import static com.googlecode.javacv.cpp.opencv_core.cvSize;
import static com.googlecode.javacv.cpp.opencv_core.cvTermCriteria;
import static com.googlecode.javacv.cpp.opencv_highgui.cvConvertImage;
import static com.googlecode.javacv.cpp.opencv_highgui.cvShowImage;
import static com.googlecode.javacv.cpp.opencv_highgui.cvWaitKey;
import static com.googlecode.javacv.cpp.opencv_imgproc.cvGoodFeaturesToTrack;
import static com.googlecode.javacv.cpp.opencv_imgproc.cvFindCornerSubPix;
import static com.googlecode.javacv.cpp.opencv_video.cvCalcOpticalFlowPyrLK;
import pl.poznan.put.cs.idss.students.kingcart.core.model.Features;

import com.googlecode.javacv.cpp.opencv_core.CvPoint;
import com.googlecode.javacv.cpp.opencv_core.CvPoint2D32f;
import com.googlecode.javacv.cpp.opencv_core.CvScalar;
import com.googlecode.javacv.cpp.opencv_core.CvSize;
import com.googlecode.javacv.cpp.opencv_core.CvTermCriteria;
import com.googlecode.javacv.cpp.opencv_core.IplImage;

@Deprecated
public class SimpleTracker implements Tracker {
	private IplImage imageToTrack;

	public IplImage track(IplImage currFrame) {
		if (imageToTrack == null)
			throw new NullPointerException("Image to track is not set!");
		// IplImage imageToTrack = IplImage.createFrom(imgToTrack);
		IplImage imageToTrack_ = cvCreateImage(cvGetSize(imageToTrack), 8, 1);

		
		// cvShowImage("IamgeToTRack", imageToTrack);
		// cvWaitKey(1000);
		cvErode(currFrame, currFrame, null, 3);
		cvDilate(currFrame, currFrame, null, 3);
		 cvShowImage("Frame", currFrame);
		 cvWaitKey(1000);


		IplImage imgCurrentCopy = cvCreateImage(cvGetSize(currFrame), IPL_DEPTH_8U, 3);
		cvConvertImage(currFrame, imgCurrentCopy, 0);
		
		
		IplImage imgToTrackCopy = cvCreateImage(cvGetSize(imageToTrack),
				IPL_DEPTH_8U, 3);
		cvConvertImage(imageToTrack, imgToTrackCopy, 0);

		CvSize img_size = cvGetSize(imageToTrack);
		int win_size = 10;

		IplImage eig_image = cvCreateImage(img_size, IPL_DEPTH_32F, 1);
		IplImage tmp_image = cvCreateImage(img_size, IPL_DEPTH_32F, 1);

		CvPoint2D32f corners = new CvPoint2D32f(500);
		int[] count = { 500 };
		cvGoodFeaturesToTrack(imageToTrack_, eig_image, tmp_image, corners,
				count, 0.05, 5.0, null, 3, 0, 0.04);


		
		// jakies Core dumpy po paru iteracjach
		// cvFindCornerSubPix(imageToTrack_, corners, 500,
		// cvSize(win_size, win_size), cvSize(-1, -1),
		// cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));

		byte optical_flow_found_feature[] = new byte[500];
		float optical_flow_feature_error[] = new float[500];
		CvTermCriteria term = cvTermCriteria(
				CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .3);

		CvPoint2D32f cornersB = new CvPoint2D32f(500);

		CvSize pyr_sz = cvSize(img_size.width() + 8, img_size.height() / 3);

		IplImage pyrA = cvCreateImage(pyr_sz, IPL_DEPTH_8U, 1);
		IplImage pyrB = cvCreateImage(pyr_sz, IPL_DEPTH_8U, 1);

		// here
		cvCalcOpticalFlowPyrLK(imgToTrackCopy, currFrame, pyrA, pyrB, corners,
				cornersB, 500, cvSize(win_size, win_size), 5,
				optical_flow_found_feature, optical_flow_feature_error,
				cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.3), 0);

		// cvZero(imgC);

		// cvShowImage( "before", imgC );
		// cvWaitKey(1000);

		for (int i = 0; i < 500; i++) {
			if (optical_flow_found_feature[i] == 0
					|| optical_flow_feature_error[i] > 550) {
			//	System.out.println("Error is " + optical_flow_feature_error[i]);
				continue;
			}
			///////////////////////////////////////////////
		//	System.out.println("Got it");
			CvPoint p0 = cvPoint(Math.round(corners.position(i).x()),
					Math.round(corners.position(i).y()));
			CvPoint p1 = cvPoint(Math.round(cornersB.position(i).x()),
					Math.round(cornersB.position(i).y()));
			// cvLine(imgC, p0, p1, CV_RGB(255, 0, 0), 2, 8, 0);
		/////////////////////////////////////////////////////////////	
//			cvCircle(currFrame,
//					p0,
//					20,
//					CV_RGB(0, 255, 0),0,0,0);
			///////////////////////////////////////////////////
			//
			int line_thickness;
			line_thickness = 1;
			/*
			 * CV_RGB(red, green, blue) is the red, green, and blue components
			 * of the color you want, each out of 255.
			 */
			CvScalar line_color;
			line_color = CV_RGB(255, 0, 0);

			double angle;
			angle = Math.atan2((double) p0.y() - p1.y(),
					(double) p0.x() - p1.x());
			double hypotenuse;
			hypotenuse = Math.sqrt(Math.pow(p0.y() - p1.y(), 2)
					+ Math.pow(p0.x() - p1.x(), 2));

			/* Here we lengthen the arrow by a factor of three. */
			// p1.x = (int) (p.x - 3 * hypotenuse * Math.cos(angle));
			// q.y = (int) (p.y - 3 * hypotenuse * Math.sin(angle));

			/* Now we draw the main line of the arrow. */
			/*
			 * "frame1" is the frame to draw on. "p" is the point where the line
			 * begins. "q" is the point where the line stops. "CV_AA" means
			 * antialiased drawing. "0" means no fractional bits in the center
			 * cooridinate or radius.
			 */
			
			cvLine(imgCurrentCopy, p0, p1, line_color, line_thickness, 8, 0);
			/*
			 * Now draw the tips of the arrow. I do some scaling so that the
			 * tips look proportional to the main line of the arrow.
			 */
			p0.put((int) (p1.x() + 9 * Math.cos(angle + Math.PI / 4)),
					(int) (p1.y() + 9 * Math.sin(angle + Math.PI / 4)));
			cvLine(imgCurrentCopy, p0, p1, line_color, line_thickness, 8, 0);
			p0.put((int) (p1.x() + 9 * Math.cos(angle - Math.PI / 4)),
					(int) (p1.y() + 9 * Math.sin(angle - Math.PI / 4)));
			cvLine(imgCurrentCopy, p0, p1, line_color, line_thickness, 8, 0);
		}
		// cvShowImage("Image", imgC);
		//cvShowImage("LKpyr_OpticalFlow", imgC);
		//cvWaitKey(1000);
		return imgCurrentCopy;

	}

	public void setImageToTrack(IplImage imageToTrack) {
		this.imageToTrack = imageToTrack;

	}

	public void setFeatures(Features features) {
		// TODO Auto-generated method stub
		
	}
}
